package main;

import static com.googlecode.javacv.cpp.opencv_core.*;
import static com.googlecode.javacv.cpp.opencv_highgui.*;
import static com.googlecode.javacv.cpp.opencv_imgproc.*;
import static main.CONSTANTS.*;

import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import java.util.ArrayList;
import java.util.Collections;
import java.util.concurrent.locks.ReentrantLock;

import com.googlecode.javacv.CanvasFrame;
import com.googlecode.javacv.FrameGrabber.Exception;
import com.googlecode.javacv.OpenCVFrameGrabber;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvRect;
import com.googlecode.javacv.cpp.opencv_core.CvScalar;
import com.googlecode.javacv.cpp.opencv_core.CvSeq;
import com.googlecode.javacv.cpp.opencv_core.IplImage;

public class FingerExtractor extends Thread {

    private int foundContourType;
    private int detectionMethod;
    private boolean shouldStop = false;
    private ArrayList<CvPoint> fingerTips;
    private IplImage cameraImage, processedImage;

//    private CanvasFrame testCanvas;

    private final OpenCVFrameGrabber grabber;
    private final IplImage twoFingersRef;
    private final IplImage leftFingerRef;
    private final ReentrantLock lock = new ReentrantLock();
    // private final IplImage rightFingerRef;
    private CvSeq refContoursTwoFingers;
    private CvSeq refContoursLeftFinger;

    // private CvSeq refContoursRightFinger;

    public FingerExtractor(OpenCVFrameGrabber grabber, int detectionMethod) {
	this.grabber = grabber;
	try {
	    grabber.start();
	} catch (Exception e1) {
	    e1.printStackTrace();
	}
	this.detectionMethod = detectionMethod;
	this.fingerTips = new ArrayList<CvPoint>();
	this.twoFingersRef = cvLoadImage("2fingersRef.bmp",
		CV_LOAD_IMAGE_GRAYSCALE);
	this.leftFingerRef = cvLoadImage("leftFingerRef.bmp",
		CV_LOAD_IMAGE_GRAYSCALE);
	// this.rightFingerRef = cvLoadImage("rightFingerRef.bmp",
	// CV_LOAD_IMAGE_GRAYSCALE);
	this.foundContourType = FOUND_UNKNOWN;
    }

    public int getFoundContourType() {
	return foundContourType;
    }

    /**
     * Method used by all children of the "Thread" class to indicate the actions
     * of the created thread.
     */
    public void run() {

	CanvasFrame imageAfterFiltering = new CanvasFrame("After filter",
		CanvasFrame.getDefaultGamma() / grabber.getGamma());
	CanvasFrame normalImage = new CanvasFrame("Normal",
		CanvasFrame.getDefaultGamma() / grabber.getGamma());
//	testCanvas = new CanvasFrame("Hist eq", CanvasFrame.getDefaultGamma()
//		/ grabber.getGamma());

	// Add a listener for the frame close event
	normalImage.addWindowListener(new WindowAdapter() {
	    public void windowClosing(WindowEvent e) {
		try {
		    grabber.stop();
		} catch (Exception e1) {
		    e1.printStackTrace();
		}
		System.exit(0); // terminates the program (all threads)
	    }
	});

	try {
	    while (!shouldStop) {
		// KeyEvent key = normalImage.waitKey(20);
		// if (key != null) {
		// int code = key.getKeyCode();
		// char keyChar = key.getKeyChar();
		// switch (code) {
		// case 37:
		// lowerCannyThreshold--;
		// System.out.print("Left: " + lowerCannyThreshold
		// + System.lineSeparator());
		// break;
		// case 39:
		// lowerCannyThreshold++;
		// System.out.print("Left: " + lowerCannyThreshold
		// + System.lineSeparator());
		// break;
		// case 38:
		// upperCannyThreshold++;
		// System.out.print("Right: " + upperCannyThreshold
		// + System.lineSeparator());
		// break;
		// case 40:
		// upperCannyThreshold--;
		// System.out.print("Right: " + upperCannyThreshold
		// + System.lineSeparator());
		// break;
		// case 65:
		// maxHueThresh--;
		// if (maxHueThresh < 0)
		// maxHueThresh = 0;
		// System.out.print("maxHueThresh: " + maxHueThresh
		// + System.lineSeparator());
		// break;
		// case 68:
		// maxHueThresh++;
		// System.out.print("maxHueThresh: " + maxHueThresh
		// + System.lineSeparator());
		// break;
		// /*
		// * case 65: erode--; if (erode < 0) erode = 0;
		// * System.out.print("Erode: " + erode +
		// * System.lineSeparator()); break; case 68: erode++;
		// * System.out.print("Erode: " + erode +
		// * System.lineSeparator()); break;
		// */
		// case 87:
		// minHueThresh++;
		// System.out.print("minHueThresh: " + minHueThresh
		// + System.lineSeparator());
		// break;
		// case 83:
		// minHueThresh--;
		// if (minHueThresh < 0)
		// minHueThresh = 0;
		// System.out.print("minHueThresh: " + minHueThresh
		// + System.lineSeparator());
		// break;
		// /*
		// * case 87: dilate++; System.out.print("Dilate: " + dilate +
		// * System.lineSeparator()); break; case 83: dilate--; if
		// * (dilate < 0) dilate = 0; System.out.print("Dilate: " +
		// * dilate + System.lineSeparator()); break;
		// */
		// }
		// System.out.println(keyChar);
		// }

		cameraImage = grabber.grab();
		cvFlip(cameraImage, cameraImage, 1);

		processedImage = analyzeImage(cameraImage);

		normalImage.showImage(cameraImage);
		imageAfterFiltering.showImage(processedImage);

		// try {
		// Thread.sleep(50);
		// } catch (InterruptedException e1) {
		// e1.printStackTrace();
		// }
	    }
	} catch (Exception e) {
	    e.printStackTrace();
	}
    }

    /**
     * Use this method to stop the thread`
     */
    public void exit() {
	shouldStop = true;
    }

    /**
     * Used to get current information about found finger tip points.
     * 
     * @return ArrayList of CvPoints indicating positions of found fingertips
     *         points.
     */
    public ArrayList<CvPoint> getFingertips() {
	// creating new ArrayList to prevent
	// ConcurrentModificationException when using an iterator
	lock.lock();
	ArrayList<CvPoint> tipsCopy = new ArrayList<CvPoint>(fingerTips.size());
	for (CvPoint point : fingerTips) {
	    CvPoint newTip = new CvPoint();
	    newTip.x(point.x());
	    newTip.y(point.y());
	    tipsCopy.add(newTip);
	}
	lock.unlock();
	return tipsCopy;
    }

    private IplImage analyzeImage(IplImage originalImage) {

	// 1. Converting the image to HSV scale:
	IplImage hsvImage = cvCloneImage(originalImage);
	cvSmooth(hsvImage, hsvImage, CV_GAUSSIAN, 7);
	cvCvtColor(hsvImage, hsvImage, CV_BGR2HSV); // conversion to HSV

	// 2. Extracting the hue element:
	cvSetImageCOI(hsvImage, 1); // select first channel (Hue)
	IplImage extractedHueImage = cvCreateImage(cvGetSize(hsvImage),
		IPL_DEPTH_8U, 1);
	cvCopy(hsvImage, extractedHueImage);
	cvEqualizeHist(extractedHueImage, extractedHueImage);

	// testCanvas.showImage(extractedHueImage);

	// 3. Finding extreme values using gray scale
	CvScalar averageIntensity;
	IplImage trueMask = cvCreateImage(cvGetSize(originalImage),
		IPL_DEPTH_8U, 1);
	// Mask for extreme values on the image.
	IplImage initialMask = cvCreateImage(cvGetSize(originalImage),
		IPL_DEPTH_8U, 1);
	cvCvtColor(originalImage, trueMask, CV_BGR2GRAY);
	cvSmooth(trueMask, trueMask, CV_GAUSSIAN, 7);
	cvInRangeS(trueMask, new CvScalar(DEFAULT_LOWER_GRAY_THRESHOLD, 0, 0,
		255), new CvScalar(DEFAULT_UPPER_GRAY_THRESHOLD, 0, 0, 255),
		initialMask);
	// Computers images' average intensity from unmasked areas.
	averageIntensity = cvAvg(trueMask, initialMask);
	// Removes some areas basing on their deviation from image average.
	cvInRangeS(trueMask, new CvScalar(averageIntensity.getVal(0) * 0.4, 0,
		0, 255), new CvScalar(averageIntensity.getVal(0) * 1.4, 0, 0,
		255), trueMask);

	// 4. Removing extreme values found:
	cvAnd(extractedHueImage, trueMask, extractedHueImage, extractedHueImage);

	// 5. Extracting the skin using the hue value (optional):
	if (detectionMethod == DETECTION_METHOD_HUE) {
	    CvScalar averageHue = cvAvg(extractedHueImage, trueMask);
	    CvScalar hueMin = new CvScalar(averageHue.getVal(0)
		    * DEFAULT_LOWER_HUE_THRESHOLD, 0, 0, 255);
	    CvScalar hueMax = new CvScalar(averageHue.getVal(0)
		    * DEFAULT_UPPER_HUE_THRESHOLD, 0, 0, 255);
	    cvInRangeS(extractedHueImage, hueMin, hueMax, extractedHueImage);
	}

	// 5. Eliminating small artifacts - reduces the number of contours
	ImageProcessing.nFoldOpen(extractedHueImage, OPEN_CLOSE_OPS_COUNT);
	ImageProcessing.nFoldClose(extractedHueImage, OPEN_CLOSE_OPS_COUNT);

	// 6. Extracting contours:
	cvCanny(extractedHueImage, extractedHueImage,
		DEFAULT_LEFT_CANNY_THRESHOLD, DEFAULT_RIGHT_CANNY_THRESHOLD, 3);

	// 7. Finding fingers
	findFingers(extractedHueImage);

	// cvSaveImage("name.bmp", extractedHueImage);
	return extractedHueImage;
    }

    private void findFingers(IplImage canniedImage) {

	CvSeq imageContours = ImageProcessing.getContours(canniedImage,
		CV_RETR_LIST, CV_LINK_RUNS);

	// cvZero(canniedImage);
	// drawAllContours(canniedImage, imageContours);
	// testCanvas.showImage(canniedImage);

	if (imageContours == null || imageContours.isNull())
	    return;

	ArrayList<Integer> indexesOfAcceptedContours = getContoursBiggerThan(
		AREA_THRESHOLD, imageContours);

	if (indexesOfAcceptedContours.size() == 0)
	    return;

	CvSeq selectedContour = findBestContourMatch(COMPARISON_THRESHOLD,
		imageContours, indexesOfAcceptedContours);

	if (foundContourType == FOUND_UNKNOWN)
	    return;

//	String foundFingerInfo = "nothing";

	updateFingerTips(selectedContour);

	if (fingerTips == null)
	    return;

	CvScalar color = CV_RGB(125, 30, 200);
	cvZero(canniedImage);
	cvDrawContours(canniedImage, selectedContour, color, color, -1,
		CV_FILLED, 8);

	for (int i = 0; i < fingerTips.size(); i++) {
	    cvDrawCircle(cameraImage, fingerTips.get(i), 10, CvScalar.BLUE, -1,
		    8, 0);
	    // System.out.println(" X value = " + fingerTips.get(i).x()
	    // + " ; Y value =" + fingerTips.get(i).y());
	}

	// switch (foundContourType) {
	// case FOUND_BOTH:
	// foundFingerInfo = "both fingers.";
	// break;
	// case FOUND_LEFT:
	// foundFingerInfo = "left finger.";
	// break;
	// // case FOUND_RIGHT:
	// // foundFingerInfo = "right finger.";
	// // break;
	// }
	// // String path = "test" + imgNumber++ + ".bmp";
	// // cvSaveImage(path, testImage);
	// System.out.println("I have found " + foundFingerInfo);
	return;
    }

    private void updateFingerTips(CvSeq selectedContour) {
	lock.lock();
	fingerTips = findTipPoints(selectedContour);
	lock.unlock();
    }

    // private void drawAllContours(IplImage image, CvSeq imageContours) {
    // CvSeq pointer = imageContours;
    // // Scanner scanner = new java.util.Scanner(System.in);
    // CvScalar color = CV_RGB(125, 30, 200);
    // while (pointer != null && !pointer.isNull()) {
    // cvDrawContours(image, pointer, color, color, -1, CV_FILLED, 8);
    // pointer = pointer.h_next();
    // // scanner.nextLine();
    // }
    // }

    private CvSeq findBestContourMatch(double comparisonThreshold,
	    CvSeq imageContours, ArrayList<Integer> indexesOfAcceptedContours) {

	double comparisonResultGeneral;
	double comparisonResultBoth;
	double comparisonResultLeft;
	// double comparisonResultRight;
	double bestMatch = comparisonThreshold;

	refContoursTwoFingers = ImageProcessing.getContours(twoFingersRef,
		CV_RETR_CCOMP, CV_CHAIN_APPROX_TC89_KCOS);
	refContoursLeftFinger = ImageProcessing.getContours(leftFingerRef,
		CV_RETR_CCOMP, CV_CHAIN_APPROX_TC89_KCOS);
	// refContoursRightFinger = ImageProcessing.getContours(rightFingerRef,
	// CV_RETR_CCOMP, CV_CHAIN_APPROX_TC89_KCOS);

	CvSeq bestContour = null;

	int prevContourIndex = 0;
	int distance2Current;

	for (int j = 0; j < indexesOfAcceptedContours.size(); j++) {
	    distance2Current = indexesOfAcceptedContours.get(j).intValue()
		    - prevContourIndex;
	    prevContourIndex = indexesOfAcceptedContours.get(j).intValue();

	    while (distance2Current > 0) {
		imageContours = imageContours.h_next();
		distance2Current--;
	    }

	    comparisonResultBoth = cvMatchShapes(imageContours,
		    refContoursTwoFingers, CV_CONTOURS_MATCH_I1, 0.0);
	    comparisonResultLeft = cvMatchShapes(imageContours,
		    refContoursLeftFinger, CV_CONTOURS_MATCH_I1, 0.0);
	    // comparisonResultRight = cvMatchShapes(imageContours,
	    // refContoursRightFinger, CV_CONTOURS_MATCH_I1, 0.0);

	    comparisonResultGeneral = Math.max(comparisonResultBoth,
		    comparisonResultLeft);
	    // comparisonResultGeneral = Math.max(comparisonResultGeneral,
	    // comparisonResultRight);

	    if (comparisonResultGeneral > bestMatch) // the lower the better
		continue;

	    bestMatch = comparisonResultGeneral;
	    bestContour = imageContours;

	    if (comparisonResultBoth == comparisonResultGeneral) {
		foundContourType = FOUND_BOTH;
	    } else if (comparisonResultLeft == comparisonResultGeneral) {
		foundContourType = FOUND_LEFT;
	    } // else {
	      // foundContourType = FOUND_RIGHT;
	      // }
	}
	if (bestContour == null || bestContour.isNull())
	    foundContourType = FOUND_UNKNOWN;
	return bestContour;
    }

    private ArrayList<Integer> getContoursBiggerThan(double areaThreshold,
	    CvSeq imageContours) {
	ArrayList<Integer> indexesOfAcceptedContours = new ArrayList<Integer>();
	CvSeq contourCopy;
	int currentIndex = -1;
	// Scanner scanner = new java.util.Scanner(System.in);

	for (contourCopy = imageContours; contourCopy != null; contourCopy = contourCopy
		.h_next()) {
	    currentIndex++;
	    double currentArea = cvContourArea(contourCopy, CV_WHOLE_ARR, 0);
	    if (currentArea < areaThreshold)
		continue;
	    indexesOfAcceptedContours.add(currentIndex);
	    // System.out.println("Area of " + currentIndex + ": " +
	    // currentArea);

	    // cvDrawContours(cameraImage, contourCopy, CvScalar.RED,
	    // CvScalar.RED, -1, CV_FILLED, 8);
	    // testCanvas.showImage(cameraImage);
	    // scanner.nextLine();
	}
	Collections.sort(indexesOfAcceptedContours);
	return indexesOfAcceptedContours;
    }

    private ArrayList<CvPoint> findTipPoints(CvSeq handContour) {

	ArrayList<CvPoint> tipPoints = new ArrayList<CvPoint>();
	CvPoint leftPoint, middlePoint, rightPoint;
	CvRect boundingRect;
	int skipDistance = handContour.total() / 10 > 0 ? handContour.total() / 10
		: 1;
	double tempAngle;

	boundingRect = cvBoundingRect(handContour, 0);
	double maxDistanceFromTop = boundingRect.y() + boundingRect.height()
		* MAX_DISTANCE_FROM_TOP / 100;

	// Scanner scanner = new java.util.Scanner(System.in);

	System.out.println("Number of countour points:" + handContour.total());

	for (int i = 0; i < handContour.total(); i++) {
	    middlePoint = new CvPoint(cvGetSeqElem(handContour,
		    (i + skipDistance) % handContour.total()));
	    if (middlePoint.y() > maxDistanceFromTop)
		continue;
	    leftPoint = new CvPoint(cvGetSeqElem(handContour, i));
	    rightPoint = new CvPoint(cvGetSeqElem(handContour,
		    (i + 2 * skipDistance) % handContour.total()));
	    tempAngle = angleBetween(leftPoint, middlePoint, rightPoint);
	    if (tempAngle < ANGLE_THRESHOLD && middlePoint.y() < leftPoint.y()
		    && middlePoint.y() < rightPoint.y()) {
		tipPoints.add(middlePoint);
	    }
	    // cvDrawCircle(cameraImage, leftPoint, 20, CvScalar.BLUE, -1, 8,
	    // 0);
	    // cvDrawCircle(cameraImage, middlePoint, 20, CvScalar.RED, -1, 8,
	    // 0);
	    // cvDrawCircle(cameraImage, rightPoint, 20, CvScalar.GREEN, -1, 8,
	    // 0);
	    // testCanvas.showImage(cameraImage);
	    // scanner.nextLine();
	}
	System.out.print("Number of tipPoints - before: " + tipPoints.size());
	tipPoints = removeExcessiveTips(tipPoints);
	System.out.println(", after: " + tipPoints.size());
	return tipPoints;
    }

    private ArrayList<CvPoint> removeExcessiveTips(ArrayList<CvPoint> tipPoints) {
	if (tipPoints.size() < 2)
	    return tipPoints;

	int currentPointIndex = 0;
	CvPoint currentPoint, nextPoint, highestPoint;
	highestPoint = null;
	ArrayList<CvPoint> remainingTips = new ArrayList<CvPoint>();

	while (currentPointIndex < tipPoints.size()) {
	    currentPoint = tipPoints.get(currentPointIndex);
	    nextPoint = tipPoints.get((currentPointIndex + 1)
		    % tipPoints.size());
	    if ((highestPoint != null && distanceBetween(highestPoint,
		    currentPoint) > DISTANCE_THRESHOLD)
		    || distanceBetween(currentPoint, nextPoint) > DISTANCE_THRESHOLD) {
		if (highestPoint != null) {
		    checkAndAddPoint(highestPoint, remainingTips);
		    highestPoint = null;
		} else {
		    checkAndAddPoint(currentPoint, remainingTips);
		}
	    } else {
		if (highestPoint == null) {
		    highestPoint = currentPoint.y() < nextPoint.y() ? currentPoint
			    : nextPoint;
		} else {
		    if (currentPoint.y() < highestPoint.y())
			highestPoint = currentPoint;
		}
	    }
	    currentPointIndex++;
	}
	if (highestPoint != null) {
	    checkAndAddPoint(highestPoint, remainingTips);
	}
	return remainingTips;
    }

    private void checkAndAddPoint(CvPoint currentPoint,
	    ArrayList<CvPoint> remainingTips) {
	
	int size = remainingTips.size();
	boolean canAdd = true;
	CvPoint analyzedPoint;
	// Compares with other points already classified for output.
	for (int i = 0; i < size; ++i) {
	    analyzedPoint = remainingTips.get(i);
	    if (distanceBetween(analyzedPoint, currentPoint) < DISTANCE_THRESHOLD) {
		canAdd = false;
		// Checks if the point already in the list is higher then the
		// new point.
		if (analyzedPoint.y() > currentPoint.y()) {
		    remainingTips.set(i, currentPoint);
		}
		break;
	    }
	}
	if (canAdd)
	    remainingTips.add(currentPoint);
    }

    private static double angleBetween(CvPoint leftPoint, CvPoint middlePoint,
	    CvPoint rightPoint) {
	double left2middleDistance = distanceBetween(leftPoint, middlePoint);
	double right2middleDistance = distanceBetween(rightPoint, middlePoint);
	double left2rightDistance = distanceBetween(leftPoint, rightPoint);
	double cosB = Math.pow(left2middleDistance, 2)
		+ Math.pow(right2middleDistance, 2)
		- Math.pow(left2rightDistance, 2);
	cosB /= 2 * left2middleDistance * right2middleDistance;
	return Math.acos(cosB) * 180 / Math.PI;
    }

    private static double distanceBetween(CvPoint p1, CvPoint p2) {
	return Math.sqrt((p1.x() - p2.x()) * (p1.x() - p2.x())
		+ (p1.y() - p2.y()) * (p1.y() - p2.y()));
    }
}